/***************************************************************************\
 * Copyright (C) by Keio University
 * OpenCVOperators.hpp created in 10 2012.
 * Mail : fdesorbi@hvrl.ics.keio.ac.jp
 *
 * OpenCVOperators.hpp is part of the HVRL Engine Library.
 *
 * The HVRL Engine Library is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * The HVRL Engine Library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
\***************************************************************************/

#ifndef HVRL_OPENCV_OPERATORS_HPP__
#define HVRL_OPENCV_OPERATORS_HPP__

#include <opencv2/opencv.hpp>
#include "hvrl/common/Common.hpp"

namespace hvrl {

cv::Vec3f operator+( const cv::Vec3f& v, const cv::Mat& m){

	if(m.cols!=1 || m.rows!=3 || m.type() != CV_32F){
		throw hvrl::Exception("operator*", "Matrix should of size 3x1 and of type float");
	}

	cv::Vec3f res;
	res[0] = m.at<float>(0,0)+v[0];
	res[1] = m.at<float>(1,0)+v[1];
	res[2] = m.at<float>(2,0)+v[2];

	return res;

}

cv::Vec4f operator+( const cv::Vec4f& v, const cv::Mat& m){

	if(m.cols!=1 || m.rows!=4 || m.type() != CV_32F){
		throw hvrl::Exception("operator*", "Matrix should of size 4x1 and of type float");
	}

	cv::Vec4f res;
	res[0] = m.at<float>(0,0)+v[0];
	res[1] = m.at<float>(1,0)+v[1];
	res[2] = m.at<float>(2,0)+v[2];
	res[3] = m.at<float>(3,0)+v[3];

	return res;

}


cv::Vec3f operator*(const cv::Mat& m, const cv::Vec3f& v){

	if(m.cols!=3 || m.rows!=3 || m.type() != CV_32F){
		throw hvrl::Exception("operator*", "Matrix should of size 3x3 and of type float");
	}

	cv::Vec3f res;
	res[0] = m.at<float>(0,0)*v[0] + m.at<float>(0,1)*v[1] + m.at<float>(0,2)*v[2];
	res[1] = m.at<float>(1,0)*v[0] + m.at<float>(1,1)*v[1] + m.at<float>(1,2)*v[2];
	res[2] = m.at<float>(2,0)*v[0] + m.at<float>(2,1)*v[1] + m.at<float>(2,2)*v[2];

	return res;

}

cv::Vec4f operator*(const cv::Mat& m, const cv::Vec4f& v){

	if(m.cols!=4 || m.rows!=4 || m.type() != CV_32F){
		throw hvrl::Exception("operator*", "Matrix should of size 4x4 and of type float");
	}

	cv::Vec4f res;
	res[0] = m.at<float>(0,0)*v[0] + m.at<float>(0,1)*v[1] + m.at<float>(0,2)*v[2] + m.at<float>(0,3)*v[3];
	res[1] = m.at<float>(1,0)*v[0] + m.at<float>(1,1)*v[1] + m.at<float>(1,2)*v[2] + m.at<float>(1,3)*v[3];
	res[2] = m.at<float>(2,0)*v[0] + m.at<float>(2,1)*v[1] + m.at<float>(2,2)*v[2] + m.at<float>(2,3)*v[3];
	res[3] = m.at<float>(3,0)*v[0] + m.at<float>(3,1)*v[1] + m.at<float>(3,2)*v[2] + m.at<float>(3,3)*v[3];

	return res;

}

cv::Vec3d operator*(const cv::Mat& m, const cv::Vec3d& v){

	if(m.cols!=3 || m.rows!=3 || m.type() != CV_64F){
		throw hvrl::Exception("operator*", "Matrix should of size 3x3 and of type float");
	}

	cv::Vec3d res;
	res[0] = m.at<double>(0,0)*v[0] + m.at<double>(0,1)*v[1] + m.at<double>(0,2)*v[2];
	res[1] = m.at<double>(1,0)*v[0] + m.at<double>(1,1)*v[1] + m.at<double>(1,2)*v[2];
	res[2] = m.at<double>(2,0)*v[0] + m.at<double>(2,1)*v[1] + m.at<double>(2,2)*v[2];

	return res;

}

cv::Vec4d operator*(const cv::Mat& m, const cv::Vec4d& v){

	if(m.cols!=4 || m.rows!=4 || m.type() != CV_64F){
		throw hvrl::Exception("operator*", "Matrix should of size 4x4 and of type float");
	}

	cv::Vec4d res;
	res[0] = m.at<double>(0,0)*v[0] + m.at<double>(0,1)*v[1] + m.at<double>(0,2)*v[2] + m.at<double>(0,3)*v[3];
	res[1] = m.at<double>(1,0)*v[0] + m.at<double>(1,1)*v[1] + m.at<double>(1,2)*v[2] + m.at<double>(1,3)*v[3];
	res[2] = m.at<double>(2,0)*v[0] + m.at<double>(2,1)*v[1] + m.at<double>(2,2)*v[2] + m.at<double>(2,3)*v[3];
	res[3] = m.at<double>(3,0)*v[0] + m.at<double>(3,1)*v[1] + m.at<double>(3,2)*v[2] + m.at<double>(3,3)*v[3];

	return res;

}

std::ostream& operator<<( std::ostream &flux, const cv::Vec3f& v ){
	flux << "[" << v[0] << "; " << v[1] << "; " << v[2] << "]";
	return flux;
}

std::ostream& operator<<( std::ostream &flux, const cv::Matx33f& m ){
	flux << "[" << m(0,0) << "; " << m(0,1) << "; " << m(0,2)  << std::endl;
	flux << m(1,0) << "; " << m(1,1) << "; " << m(1,2) << std::endl;
	flux << m(2,0) << "; " << m(1,2) << "; " << m(2,2) << "]";
	return flux;
}


}

#endif
